#ifndef __SCAAR_DRIVER_H__
#define __SCAAR_DRIVER_H__

#include <vector>
#include <unordered_map>
#include <string>
#include "math.h"
#include "time.h"
#include <ros/ros.h>
#include "message/ScaarMsg.h"
#include "message/LowLevelCmd.h"
#include "message/LowLevelState.h"
#include <thread>
#include "serial/serial.h"

namespace arm_robot_hardware_drivers {
class ScaarDriver {
  public:
    void init(std::string port, int baudrate, int num_joints);
    void update(std::vector<double>& pos_commands, std::vector<double>& joint_positions);
    void getJointPositions(std::vector<double>& actuator_positions);
    void calibrateJoints();

    float MRC_voltage[2];
    float SA100angle_;

    ScaarDriver();
    ~ScaarDriver();

  private:
    bool initialised_;
    std::string version_;

    bool received_successfully_;

    int num_joints_;
    SCA_ARM_FBK_MSG inMsg_;
    int inMsgSize_;
    int outMsgSize_;
    SCA_ARM_CMD_MSG outMsg_;
    int mode_;
    serial::Serial serial_;

    std::vector<MotorCmd> motorcmd_;
    std::vector<MotorState> motorstate_;

    // Comms with scaar
    void exchange(); // exchange joint commands/state
    bool transmit();
    bool receive();
    bool extractInMsg();
    void motifyOutMsg();
    void updatePosState(std::vector<double>& joint_positions);
    void updatePosCmd(std::vector<double>& pos_commands);
    void printOutMsgInHex();
    void printInMsgInHex();
    void sendCommand(); // send arbitrary commands
    void setWork_Kp_Kd();
    void checkInit();

};

} // namespace arm_robot_hardware_drivers

#endif // __SCAAR_DRIVER_H__